{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 设置3种自由度的配准结果\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "\n",
    "\n",
    "class Homography_8:\n",
    "\n",
    "    get_x = lambda x, y, H: (x * H[0, 0] + y * H[0, 1] + H[0, 2])/(x * H[2, 0] + y * H[2, 1] + 1.0)\n",
    "    get_y = lambda x, y, H: (x * H[1, 0] + y * H[1, 1] + H[1, 2])/(x * H[2, 0] + y * H[2, 1] + 1.0)\n",
    "\n",
    "    class error(Exception):\n",
    "        def __init__(self, message):\n",
    "            \"\"\"\n",
    "            自定义异常\n",
    "            :param message: 抛出异常时的提示\n",
    "            \"\"\"\n",
    "            self.message = message\n",
    "            super().__init__(self.message)\n",
    "\n",
    "    def __init__(self, pts_reg: np.ndarray, pts_ref: np.ndarray) -> None:\n",
    "        if pts_reg.shape[0] != pts_ref.shape[0]:\n",
    "            raise Homography_8.error('配准点和参考点的数量不等')\n",
    "        if pts_reg.shape[0] == 0:\n",
    "            raise Homography_8.error('没有设置特征点')\n",
    "        if pts_reg.shape[1] != 2:\n",
    "            raise Homography_8.error('配准点必须是二维的')\n",
    "        if pts_ref.shape[1] != 2:\n",
    "            raise Homography_8.error('参考点必须是二维的')\n",
    "        self.num = pts_reg.shape[0]\n",
    "        self.pts_reg = pts_reg\n",
    "        self.pts_ref = pts_ref\n",
    "\n",
    "    def get_homography(self):\n",
    "        \"\"\"\n",
    "        解析单应性矩阵\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        import cv2 as cv\n",
    "        if self.num < 4:\n",
    "            raise Homography_8.error('配准点的数量必须大于等于4')\n",
    "        H, _ = cv.findHomography(self.pts_reg, self.pts_ref)\n",
    "        self.H = H\n",
    "\n",
    "    def get_error(self) -> pd.DataFrame:\n",
    "        \"\"\"\n",
    "        计算配准误差\n",
    "        :return: 返回误差\n",
    "        \"\"\"\n",
    "        result = []\n",
    "        for i in range(self.num):\n",
    "            trans_x = Homography_8.get_x(self.pts_reg[i, 0], self.pts_reg[i, 1], self.H)\n",
    "            trans_y = Homography_8.get_y(self.pts_reg[i, 0], self.pts_reg[i, 1], self.H)\n",
    "            error_x = trans_x - self.pts_ref[i, 0]\n",
    "            error_y = trans_y - self.pts_ref[i, 1]\n",
    "            result.append([self.pts_reg[i, 0], self.pts_reg[i, 1], trans_x, trans_y,\n",
    "                           self.pts_ref[i, 0], self.pts_ref[i, 1], error_x, error_y])\n",
    "        result: pd.DataFrame = pd.DataFrame(result, \n",
    "                                            columns=['reg_x', 'reg_y', 'trans_x', 'trans_y', 'ref_x', 'ref_y', 'error_x', 'error_y'])\n",
    "        return result\n",
    "\n",
    "\n",
    "class Homography_6(Homography_8):\n",
    "    def __init__(self, pts_reg: np.ndarray, pts_ref: np.ndarray) -> None:\n",
    "        super().__init__(pts_reg, pts_ref)\n",
    "\n",
    "    def get_homography(self):\n",
    "        \"\"\"\n",
    "        计算自由度为6的单应性矩阵\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        if self.num < 3:\n",
    "            raise Homography_8.error('配准点的数量必须大于等于3')\n",
    "        num = self.num\n",
    "        A = np.zeros([2 * num, 6], dtype=np.float64)\n",
    "        b = np.zeros([2 * num], dtype=np.float64)\n",
    "        for i in range(num):\n",
    "            A[2 * i, 0] = self.pts_reg[i, 0]\n",
    "            A[2 * i, 1] = self.pts_reg[i, 1]\n",
    "            A[2 * i, 2] = 1.0\n",
    "            b[2 * i] = self.pts_ref[i, 0]\n",
    "            A[2 * i + 1, 3] = self.pts_reg[i, 0]\n",
    "            A[2 * i + 1, 4] = self.pts_reg[i, 1]\n",
    "            A[2 * i + 1, 5] = 1.0\n",
    "            b[2 * i + 1] = self.pts_ref[i, 1]\n",
    "        result = np.linalg.lstsq(A, b)\n",
    "        h = result[0]\n",
    "        H = np.zeros([3, 3], dtype=np.float64)\n",
    "        H[0, :] = h[: 3]\n",
    "        H[1, :] = h[3:]\n",
    "        H[2, 2] = 1.0\n",
    "        self.H = H\n",
    "\n",
    "\n",
    "class Homography_4(Homography_8):\n",
    "    def __init__(self, pts_reg: np.ndarray, pts_ref: np.ndarray) -> None:\n",
    "        super().__init__(pts_reg, pts_ref)\n",
    "    \n",
    "    def get_homography(self):\n",
    "        \"\"\"\n",
    "        解析配准量，包括缩放量、旋转量（角度制）、平移量，并计算单应性矩阵\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        from numpy import cos, sin\n",
    "        if self.num < 2:\n",
    "            raise Homography_8.error('配准点的数量必须大于等于2')\n",
    "        # 计算两组坐标的中心点\n",
    "        center_reg = np.mean(self.pts_reg, axis=0)\n",
    "        center_ref = np.mean(self.pts_ref, axis=0)\n",
    "        # 去中心化\n",
    "        decentered_reg = self.pts_reg - center_reg\n",
    "        decentered_ref = self.pts_ref - center_ref\n",
    "        # 将去中心后的坐标转换为复数形式\n",
    "        complex_reg = decentered_reg[:, 0] + 1j * decentered_reg[:, 1]\n",
    "        complex_ref = decentered_ref[:, 0] + 1j * decentered_ref[:, 1]\n",
    "        # 计算两组复数序列的离散傅里叶变换（DFT）\n",
    "        fft_reg = np.fft.fft(complex_reg)\n",
    "        fft_ref = np.fft.fft(complex_ref)\n",
    "        fft_reg_conj = np.conjugate(fft_reg)  # 计算两组DFT结果的共轭\n",
    "        cross_power_spectrum = np.multiply(fft_reg_conj, fft_ref)  # 计算互功率谱（点乘共轭）\n",
    "        # cross_power_spectrum = np.divide(cross_power_spectrum, np.abs(fft_reg))\n",
    "        # cross_power_spectrum = np.divide(cross_power_spectrum, np.abs(fft_reg))\n",
    "        index_max = np.argmax(np.abs(cross_power_spectrum))  # 找到相位相关峰值的位置，这将是位移的估计\n",
    "        angle = np.angle(cross_power_spectrum[index_max], deg=False)  # 计算相位（弧度制）\n",
    "        scale = np.abs(cross_power_spectrum[index_max]) / np.abs(fft_reg[index_max]) ** 2\n",
    "        shift_x = -scale * cos(angle) * center_reg[0] + scale * sin(angle) * center_reg[1] + center_ref[0]\n",
    "        shift_y = -scale * sin(angle) * center_reg[0] - scale * cos(angle) * center_reg[1] + center_ref[1]\n",
    "        H = np.array([scale * cos(angle), -scale * sin(angle), shift_x,\n",
    "                      scale * sin(angle), scale * cos(angle), shift_y,\n",
    "                      0, 0, 1.0])\n",
    "        angle = np.rad2deg(angle)\n",
    "        angle %= 360\n",
    "        self.angle = angle\n",
    "        self.scale = scale\n",
    "        self.shift_x = shift_x\n",
    "        self.shift_y = shift_y\n",
    "        self.H = H.reshape([3, 3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def set_homography(arg: np.ndarray):\n",
    "    \"\"\"\n",
    "    根据参数设置单应性矩阵。（自由度分别为4/6/8）\n",
    "    :param arg: 生成单应性矩阵的参数\n",
    "    :return:\n",
    "    \"\"\"\n",
    "    from numpy import cos, sin\n",
    "    if arg.shape[0] == 4:\n",
    "        scale = arg[0]\n",
    "        angle = arg[1]\n",
    "        angle = np.deg2rad(angle)\n",
    "        shift_x = arg[2]\n",
    "        shift_y = arg[3]\n",
    "        H = np.array([scale * cos(angle), -scale * sin(angle), shift_x,\n",
    "                      scale * sin(angle), scale * cos(angle), shift_y,\n",
    "                      0, 0, 1.0])\n",
    "        H = H.reshape([3, 3])\n",
    "    if arg.shape[0] == 6:\n",
    "        H = np.zeros([3, 3], dtype=np.float64)\n",
    "        H[0, :] = arg[: 3]\n",
    "        H[1, :] = arg[3:]\n",
    "        H[2, 2] = 1.0\n",
    "    if arg.shape[0] == 8:\n",
    "        H = np.zeros([3, 3], dtype=np.float64)\n",
    "        H[0, :] = arg[: 3]\n",
    "        H[1, :] = arg[3: 6]\n",
    "        H[2, : 2] = arg[6:]\n",
    "        H[2, 2] = 1.0\n",
    "    return H\n",
    "\n",
    "\n",
    "def generate_pt(num: int, H: np.ndarray, is_noise=True):\n",
    "    \"\"\"\n",
    "    生成配准点和参考点，默认加入噪声\n",
    "    :param num: 随机点的数量\n",
    "    :param H: 单应性矩阵\n",
    "    :param is_noise: 是否加入噪声\n",
    "    :return:\n",
    "    \"\"\"\n",
    "    pt_reg = np.random.uniform(0, 1000, (num, 2))\n",
    "    pt_ref = np.zeros_like(pt_reg)\n",
    "    for i in range(num):\n",
    "        pt_ref[i, 0] = Homography_8.get_x(pt_reg[i, 0], pt_reg[i, 1], H)\n",
    "        pt_ref[i, 1] = Homography_8.get_y(pt_reg[i, 0], pt_reg[i, 1], H)\n",
    "    if is_noise:\n",
    "        noise = np.random.normal(0, 2, pt_ref.shape)\n",
    "        pt_ref += noise\n",
    "    return pt_reg, pt_ref\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    arg = np.array([2, 30, 12, 16])  # 幅度、角度、平移量\n",
    "    print(arg)\n",
    "    H_set = set_homography(arg)\n",
    "    print(H_set)\n",
    "    pt_reg, pt_ref = generate_pt(8, H_set, is_noise=True)\n",
    "    registration = Homography_4(pt_reg, pt_ref)\n",
    "    registration.get_homography()\n",
    "    print(registration.get_error())\n",
    "    print(registration.scale, registration.angle, registration.shift_x, registration.shift_y)\n",
    "    print(registration.H)\n",
    "\n",
    "    # arg = np.array([2.1, 2.6, 8.1, 2.3, 2.5, 7.4])\n",
    "    # print(arg)\n",
    "    # H_set = set_homography(arg)\n",
    "    # print(H_set)\n",
    "    # pt_reg, pt_ref = generate_pt(8, H_set, is_noise=True)\n",
    "    # registration = Homography_6(pt_reg, pt_ref)\n",
    "    # registration.get_homography()\n",
    "    # print(registration.get_error())\n",
    "    # print(registration.H)\n",
    "\n",
    "    # arg = np.array([2.1, 2.6, 8.1, 2.3, 2.5, 7.4, 1e-3, -2e-3])\n",
    "    # print(arg)\n",
    "    # H_set = set_homography(arg)\n",
    "    # print(H_set)\n",
    "    # pt_reg, pt_ref = generate_pt(8, H_set, is_noise=False)\n",
    "    # registration = Homography(pt_reg, pt_ref)\n",
    "    # registration.get_homography()\n",
    "    # print(registration.get_error())\n",
    "    # print(registration.H)\n",
    "    \n",
    "    pass\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "project_main2",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.20"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
